#include "MyRobot.h"
#include "VisualData.h"
#include "RobotUtils.h"
#include "KinectMotor.h"

using namespace cv;

MyRobot::MyRobot(void) {

	//	leggo la posizone iniziale del robot rispetto al sistema globale
	RobotUtils::readInitialPose(initialPose);

	//	matrice di traslazione e rotazione del sistema robot rispetto al sistema globale
	double thetaRad = initialPose.getTh() * CV_PI / (double) 180;
	cv::Mat robotToGlobal = (cv::Mat_<double>(3, 3) <<
		cos(thetaRad),		-sin(thetaRad),		initialPose.getX(),
		sin(thetaRad),		cos(thetaRad),		initialPose.getY(),
		0,					0,						1);

	//	matrice di traslazione e rotazione del sistema globale rispetto al sistema robot
	globalToRobot = robotToGlobal.inv();

	// Goto action at lower priority
	addAction(&gotoPoseAction, 50);
}

MyRobot::~MyRobot(void) {

	//	il problema del crash finale sta qui!
	Aria::shutdown();
	exit(0);
}

/**	Raggiunge una sequenza di obiettivi (punti sul piano del sistema globale)
*	
*	@param goals vettore degli obiettivi da raggiungere
*/
void MyRobot::reachGoals(const std::vector<ArPose> &goals) {
	//	X	->	coordinate omogenee globali
	//	Xr	->	coordinate omogenee robot
	cv::Mat_<double> X(3, 1), Xr(3, 1);
	for (std::vector<ArPose>::const_iterator goal = goals.begin(); goal != goals.end(); ++goal) {
		X(0, 0) = goal->getX();
		X(1, 0) = goal->getY();
		X(2, 0) = 1;

		//	traduzione da coordinate globali a coordinate robot
		Xr = globalToRobot * X;

		lock();

		ArLog::log(ArLog::Normal, "Going to (%f, %f)\n", Xr(0, 0), Xr(1, 0));
		gotoPoseAction.setGoal(ArPose(Xr(0, 0), Xr(1, 0)));

		unlock();

		while (!gotoPoseAction.haveAchievedGoal())
			ArUtil::sleep(100);
	}
}

void MyRobot::reachGoalsAndCorrectPose(const std::vector<ArPose> &goals, int CORRECTION_INTERVAL)
{
	//	intervallo di correzione minimo: 100 ms
	if(CORRECTION_INTERVAL < 100)
		CORRECTION_INTERVAL = 100;

	//	timer utilizzato per scandire le correzioni
	ArTime timer;
	timer.setToNow();

	//	oldPose			->	posizione rilevata dai dati odometrici
	//	correctedPose	->	posizione corretta coi dati visuali
	ArPose oldPose;
	ArPose correctedPose;

	//	X			->	coordinate omogenee globali dell'obiettivo
	//	Xr			->	coordinate omogenee robot dell'obiettivo
	Mat_<double>	X(3,1);
	Mat_<double>	Xr(3,1);

	

	//	ciclo su tutti gli obiettivi da raggiungere
	for(std::vector<ArPose>::const_iterator goal = goals.begin(); goal != goals.end(); ++goal)
	{
		X(0,0) = goal->getX();
		X(1,0) = goal->getY();
		X(2,0) = 1;

		//	traduzione da coordinate globali a coordinate robot
		Xr = globalToRobot * X;

		//	imposto il prossimo obiettivo
		lock();
		ArLog::log(ArLog::Normal,"MyRobot::reachGoalsAndCorrectPose: Going to (%f,%f)",
			Xr(0,0),Xr(1,0));
		gotoPoseAction.setGoal(ArPose(Xr(0,0),Xr(1,0)));
		unlock();

		//	finche' non ho raggiunto l'obiettivo
		while(!gotoPoseAction.haveAchievedGoal())
		{
			//	se e' trascorso l'intervallo di correzione,
			//	procedo rilevando i marker
			if(timer.mSecSince() > CORRECTION_INTERVAL)
			{
				//	mi dice se ho trovato almeno un marker oppure no
				if(visualData.getGlobalPose(correctedPose))
				{
					//	ottenute posizione ed orientazione della telecamera rispetto al sistema globale,
					//	le riporto al sistema robot sfruttando la posizione iniziale del robot
					//	P_ROBOT		->	coordinate omogenee del robot rispetto al sistema globale
					//	P_ROBOTr	->	coordinate omogenee del robot rispetto al sistema robot
					cv::Mat_<double> P_ROBOT(3,1);
					cv::Mat_<double> P_ROBOTr(3,1);

					P_ROBOT(0,0) = correctedPose.getX();
					P_ROBOT(1,0) = correctedPose.getY();
					P_ROBOT(2,0) = 1;

					//	traduzione da coordinate globali a coordinate robot
					P_ROBOTr = globalToRobot * P_ROBOT;

					correctedPose.setX(P_ROBOTr(0,0));
					correctedPose.setY(P_ROBOTr(1,0));
					correctedPose.setTh(correctedPose.getTh() - initialPose.getTh());

					ArLog::log(ArLog::Normal,
						"MyRobot::reachGoalsAndCorrectPose: correctedPose:	(%f,%f,%f)",
						correctedPose.getX(),correctedPose.getY(),correctedPose.getTh());

					//	salvo la posizione rilevata dai dati odometrici
					oldPose = getPose();

					ArLog::log(ArLog::Normal,
						"Correcting pose from (%f, %f, %f) to (%f, %f, %f)\n",
						oldPose.getX(),oldPose.getY(),oldPose.getTh(),
						correctedPose.getX(),correctedPose.getY(),correctedPose.getTh());

					lock();
					gotoPoseAction.cancelGoal();
					moveTo(correctedPose);
					//	correzione della posizione memorizzata
					ArUtil::sleep(1000);
					gotoPoseAction.setGoal(ArPose(Xr(0,0),Xr(1,0)));
					unlock();
				}
				else
				{
					ArLog::log(ArLog::Normal,
						"MyRobot::reachGoalsAndCorrectPose: %d ms passed but no marker found!",
						timer.mSecSince());
				}

				//	sia che abbia trovato il marker oppure no, resetto il timer
				timer.setToNow();
			}

			//	in ogni caso aspetto 100 ms prima della prossima iterazione
		}
	}
	cout << "frame counter: " << visualData.framecounter << endl;
}
